// SPDX-License-Identifier: GPL-2.0+
/*
 * Copyright (C) 2023-2024, Phytium Technology Co., Ltd.
 * This file describes the initialization of the
 * Basic function of Phytium pe2201 board.
 */

#include <common.h>
#include <asm/system.h>
#include <asm/io.h>
#include <asm/gic.h>
#include <asm/armv8/mmu.h>
#include <linux/arm-smccc.h>
#include <netdev.h>
#include <e_uart.h>
#include "../cpu.h"
#include "board.h"
#include "../power_manage/power_manage.h"
#include "../parameter/parameter.h"

DECLARE_GLOBAL_DATA_PTR;

/*
 * set the sck frequency of QSPI
 */
void inline set_freq_div(uint8_t rank)
{
	uint32_t temp;

	if (rank > 7)
		rank = 7;

	temp = readl(QSPI_ADDR_READ_CFG_REG);
	temp = (temp & (~0x7)) | rank;
	
	/* configure READ_CFG_REF to realize qspi div */
	writel(temp, QSPI_ADDR_READ_CFG_REG);
}

/* 
 * set the rate for connecting qspi external flash
 */
void set_flash_speed(void)
{
	uint32_t speed_rank;

	speed_rank = pm_get_qspi_freq_rank();
	p_printf("get flash speed = %d\n", speed_rank);

	speed_rank += 3;
	set_freq_div(speed_rank);
	p_printf("flash set done\n");
}

typedef struct qspi_ops_s {
	uint32_t id;
	void (*fun)(void);
	uint8_t str[16];
} qspi_ops_t;

/*
 * QSPI flash QUAD set
 */
void w25q128fw_config(void)
{
	p_printf("qspi flash quad set!\n");
	writel(0x06000000, QSPI_CMD_PORT_REG);
	writel(0x1, QSPI_LD_PORT_REG);

	writel(0x31402000, QSPI_CMD_PORT_REG);
	writel(0x2, QSPI_LD_PORT_REG);

	writel((0x6B << 24) | (0x24 << 16) | (0x7 << 4) | (1 << 3), QSPI_ADDR_READ_CFG_REG);

	writel(0x04000000, QSPI_CMD_PORT_REG);
	writel(0x1, QSPI_LD_PORT_REG);
}

const qspi_ops_t qspi_ops[] = {
	{0x01182001, NULL, "flash debug"},
	{0x001860ef, w25q128fw_config, "w25q128fw"},
	{0x001960ef, w25q128fw_config, "w25q128fw"},
	{0x001840ef, w25q128fw_config, "w25q128"},
	{0x0, NULL, "default flash"}
};

/*
 * get qspi ops, match qspi flash type based on jedec_id
 */
uint32_t get_qspi_ops(void)
{
	uint32_t jedec_id, i;

	writel(0x9F002400, QSPI_CMD_PORT_REG);
	dsb();
	isb();
	jedec_id = readl(QSPI_LD_PORT_REG);
	for (i = 0; qspi_ops[i].id != 0; i++) {
		if (qspi_ops[i].id == jedec_id)
			break;
	}
	p_printf("flash type = %s\n", qspi_ops[i].str);
	return i;
}

/*
 * set the speed, type and capacity of qspi flash
 */
void qspi_flash_setup(void)
{
	void (*ops_fun)(void);
	uint32_t ops_id;

	ops_id = get_qspi_ops();

	if (ops_id != 0) {
		ops_fun = qspi_ops[ops_id].fun;

		if (ops_fun != NULL)
			ops_fun();
		set_flash_speed();
	}
}

u32 get_reset_source(void)
{
	struct arm_smccc_res res;

	arm_smccc_smc(CPU_GET_RST_SOURCE, 0, 0, 0, 0, 0, 0, 0, &res);
	p_printf("reset source = %lx \n", res.a0);

	return res.a0;
}

/*
 * detect the resource of the system
 */
void check_reset(void)
{
	u32 rst;

	qspi_flash_setup();
	rst = get_reset_source();
	gd->reset_flag = rst;

	switch (rst) {
	case CPU_RESET_POWER_ON:
		p_printf("power on reset done!\n");
		pll_init();
		break;
	case CPU_RESET_WATCH_DOG:
		p_printf("wdt reset done!\n");
		break;
	case CPU_RESET_CORE:
		p_printf("core reset done!\n");
		break;
	default:
		p_printf("other reset source\n");
		while (1);
	}
}

/* 
 * use gpio output mode to achieve device reset function
 */
void gpio_reset_device(int dev_id, int level)
{
	switch (dev_id) {
	case 0x0:                           //pcie
		if (level) {
			p_printf("pcie reset high\n");
#ifdef PE2201_BMC_SYS                   //gpio5_0
			//Configure GPIO_SWPORTA_DDR as output
			writel((0x1 | readl((size_t)(GPIO5_BASE + GPIO_SWPORTA_DDR))),
				(size_t)(GPIO5_BASE + GPIO_SWPORTA_DDR));
			//output high level
			writel((0x1 | readl((size_t)(GPIO5_BASE + GPIO_SWPORTA_DR))),
				(size_t)(GPIO5_BASE + GPIO_SWPORTA_DR));
#else                                   //gpio2_9
			//configure GPIO_SWPORTA_DDR as output
			writel((0x200 | readl((size_t)(GPIO2_BASE + GPIO_SWPORTA_DDR))),
				(size_t)(GPIO2_BASE + GPIO_SWPORTA_DDR));
			//output high level
			writel((0x200 | readl((size_t)(GPIO2_BASE + GPIO_SWPORTA_DR))),
				(size_t)(GPIO2_BASE + GPIO_SWPORTA_DR));
#endif
			mdelay(50);
		} else {
			p_printf("pcie reset low\n");
#ifdef PE2201_BMC_SYS                   //gpio5_0
			//configure GPIO_SWPORTA_DDR as output
			writel((0x1 | readl((size_t)(GPIO5_BASE + GPIO_SWPORTA_DDR))),
				(size_t)(GPIO5_BASE + GPIO_SWPORTA_DDR));
			//output low level
			writel((0xfffe | readl((size_t)(GPIO5_BASE + GPIO_SWPORTA_DR))),
				(size_t)(GPIO5_BASE + GPIO_SWPORTA_DR));
#else                                   //gpio2_9
			//configure GPIO_SWPORTA_DDR as output
			writel((0x200 | readl((size_t)(GPIO2_BASE + GPIO_SWPORTA_DDR))),
				(size_t)(GPIO2_BASE + GPIO_SWPORTA_DDR));
			//output low level
			writel((0xfdff | readl((size_t)(GPIO2_BASE + GPIO_SWPORTA_DR))),
				(size_t)(GPIO2_BASE + GPIO_SWPORTA_DR));
#endif
		}
		break;
	case 0x1:
	default:
		p_printf("unknown device!!!please check input argv!\n");
	}
}

/*
 * Configure pwm realted register to realize the func of bmc heart beat
 */
void bmc_heart_beat(void)
{
	/* configure div */
	writel(0x06600002, PWM_TIM_CTRL_REG);
	/* configure period */
	writel(0x5880, PWM_PERIOD_REG);
	writel(0xc4, PWM_CTRL_REG);
	/* configure cycle */
	writel(0x2c40, PWM_CCR_REG);
}

/* clear jpeg interrupt */
void bmc_clear_jpeg_int(void)
{
	if (gd->reset_flag == 0x03 || gd->reset_flag == 0x80) {
		/* config TACHO30 mode is timer, config counting mode and timing mode */
		writel(0x0, TACHO30 + TIMER_TACHO_CTL);
		/* config TACHO30 interrupt status is once_intr_in */
		writel(0x8, TACHO30 + TIMER_TACHO_INTR_STS);
		/* config TACHO31 mode is timer, config counting mode and timing mode */
		writel(0x0, TACHO31 + TIMER_TACHO_CTL);
		/* config TACHO31 interrupt status is once_intr_in */
		writel(0x8, TACHO31 + TIMER_TACHO_INTR_STS);
	}
}

/*
 * phy init set, configure the required rate, mode, and other realted register
 */
int pe2201_phy_init(void)
{
	uint32_t phy_domain = 0x3f;
	uint32_t phy_sel = pm_get_phy_sel_all();
	uint32_t mode = pm_get_mac_mode_all() << 8;
	uint32_t speed = pm_get_speed_mode_all() << 8;
	struct arm_smccc_res res;

	debug("phy_sel = 0x%x\n", phy_sel);
	arm_smccc_smc(SET_PHY_MODE, phy_domain, phy_sel, mode, speed, 0, 0, 0, &res);
	if (res.a0 != 0) {
		p_printf("set phy error ret  %lx\n", res.a0);
		while (1);
	}

	return 0;
}

/*
 * get pad info, realize pin ctrl function
 */
#ifdef PHYTIUM_PINCTRL_TOOLS
int pe2201_pin_ctrl(void)
{
	int groups, size;
	int pad_base;
	uint32_t buffer[384] = {0};//max 1.5k

	pad_base = PLAT_PAD_BASE;
	/* get pad info from parameter table */
	if (readl((size_t)(pad_base)) != PARAMETER_PAD_MAGIC) {
		p_printf("ERROR:board pad info no found!!!\n");
		return -1;
	}

	/* get the value and size of space allocated by the pad */
	groups = readl((size_t)(pad_base + 0x4));
	size = groups * 2 * (sizeof(int));
	memcpy((void *)buffer, (void *)((size_t)(pad_base + 0x8)), size);
	for (int i = 0; i < groups; i++)
		writel(buffer[i*2+1], (u64)buffer[i*2]);

	p_printf("parameter pad_base = 0x%x\n", pad_base);
	for (int j = 0; j < groups; j++)
		p_printf("value = 0x%x, addr = 0x%x\n", buffer[j*2+1], buffer[j*2]);

	return 0;
}

/*
 * select the MIO function is uart or i2c by configure mio_func_sel_reg
 */
void mio_func_sel(void)
{
	uint32_t mio_sel;

	mio_sel = pm_get_param_mio_sel();
	debug("mio_sel = 0x%x\n", mio_sel);

	for (int i = 0; i < 16; i++) {
		if ((mio_sel >> i) & 0x1)
			/* uart */
			writel(MIO_UART, (size_t)((MIO00 + i * 0x2000) + MIO_CTRL_OFFSET + MIO_FUNC_SEL));
		else
			/* I2C */
			writel(MIO_I2C, (size_t)((MIO00 + i * 0x2000) + MIO_CTRL_OFFSET + MIO_FUNC_SEL));
	}
}

void bmc_init(void)
{
	int ret;

	/* PWM all enable */
	writel(0xff, LSD_PWM_REG);

	if (gd->reset_flag != 0x03) {
		/* DP setup */
		writel(0x3d0, DP_DST_ADDR);
	}
	/* KCS setup */
	writel(0x01, KCS_ENABLE_REG);
	/* reset PHY */
	writel(0x01, GPIO0_BASE + GPIO_SWPORTA_DDR);
	writel(0x00, GPIO0_BASE + GPIO_SWPORTA_DR);
	udelay(2000);
	writel(0x01, GPIO0_BASE + GPIO_SWPORTA_DDR);
	//pcie ep no x100
	writel(0x8000, PEU_PSU_C0 + CONFIG_PEU_PSU_C0);

	pwr_bmc_gpio_core_reset_handler();

	writel(0x0, QSPI_CMD_PORT_REG);
	dsb();
	isb();
	printf("clear qspi cmd reg!\n");

	if (gd->reset_flag == 0x03) {
		//reset gpio
		ret = readl(LSD_BASE + CREG_LSD_RST_CONFIG3);
		ret &= ~(0x3f00);
		writel(ret, LSD_BASE + CREG_LSD_RST_CONFIG3);
		ret |= (0x3f00);
		writel(ret, LSD_BASE + CREG_LSD_RST_CONFIG3);
#ifdef BMC_EP_NO_RESET
		//no reset pcie ep
		writel(0xffffffff, PEU_PSU_C0 + LPI_CTR_COUNTER3);
#endif
	}

	return;
}

/*
 * AP sends commands to the SCP via SCMI
 * to delay the JPEG reset by 100ms
 */
void scmi_bmc_jpeg_reset_delay(void)
{
	p_printf("jpeg reset delay 100ms\n");

	/* Disable the interruption of MBX channel 0 */
	writel(0x1, AP_UBOOT_CONFIG);

	volatile mailbox_mem_e_t *mbx_mem = (mailbox_mem_e_t *)AP_TO_SCP_SHARED_MEM_BASE;

	/* build mailbox message */
	mbx_mem->msg_header = SCMI_MSG_CREATE_E(0x81, 0x0c, 0);
	mbx_mem->len = sizeof(mbx_mem->msg_header);
	mbx_mem->flags = 0x0;
	mbx_mem->status = 0x0;
	/* Initiate virtual channel communication */
	writel(mbx_mem->msg_header, AP_UBOOT_SET);
}

/*
 * used for peripheral virtualization to translate virtual and real addresses
 */
void bmc_smmu_setup(void)
{
	struct arm_smccc_res res;

	p_printf("bmc smmu setup\n");
	arm_smccc_smc(SET_SMMU_DOMAIN, 0x1, 0x1, 0, 0, 0, 0, 0, &res);
	if (res.a0 != 0) {
		p_printf("set smmu error ret  %lx\n", res.a0);
		while (1);
	}
}

/* 
 * Firmware phase configuration of
 * SATA1 controller initialization in GSD
 */
void gsd_sata_setup(void)
{
	writel(0x08000000, (size_t)(GSD_SATA_BASE + SATA_CAP));
	writel(0x00000001, (size_t)(GSD_SATA_BASE + SATA_PI));
	writel(0x04000000, (size_t)(GSD_SATA_BASE + SATA_P_CMD));
}

/*
 * Firmware phase configuration of
 * SATA0 controller initialization in PSU
 */
void psu_sata_setup(void)
{
	/* support sss(staggered spin-up) */
	writel(0x08000000, (size_t)(PSU_SATA_BASE + SATA_CAP));
	/* port implemented, ports can be used by softwore */
	writel(0x00000001, (size_t)(PSU_SATA_BASE + SATA_PI));
	/* Aggressive link power management enable */
	writel(0x04000000, (size_t)(PSU_SATA_BASE + SATA_P_CMD));
}

void onewire_init_setup(void)
{
	/* configure onewire mode */
	writel(0x10, ONEWIRE_BASE);
}

/*
 * usb init setup
 */
void usb_init_setup(void)
{
	/* usb 2_0 overcurrent_n signal process */
	writel(0x3, USB2_0_UIB + OVERCURRENTN_CTRL_REG);
	mdelay(1);
	/* usb 2_0 interrupt require */
	writeb(0x3, USB2_0_BASE + HCUSBIRQ_REG);
	mdelay(1);
	/* usb 2_1 overcurrent_n signal process */
	writel(0x3, USB2_1_UIB + OVERCURRENTN_CTRL_REG);
	mdelay(1);
	/* usb 2_1 interrupt require */
	writeb(0x3, USB2_1_BASE + HCUSBIRQ_REG);
	mdelay(1);

	writel(readl(USB2_0_PHY + SECSID_ATST_USB2_REG) | 0x80000, USB2_0_PHY + SECSID_ATST_USB2_REG);
	writel(readl(USB2_1_PHY + SECSID_ATST_USB2_REG) | 0x80000, USB2_1_PHY + SECSID_ATST_USB2_REG);

	/* usb 2_0 reference clock select */
	writel(0x0, USB2_0_UIB + USB2_PHY_REFCLK_MODE);
	/* usb 2_1 reference clock select */
	writel(0x0, USB2_1_UIB + USB2_PHY_REFCLK_MODE);
}

/*
 * vhub init setup
 */
void vhub_init_setup(void)
{
	if (!(pm_get_param_mio_sel() & 0x10000)) {
		p_printf("vhub device set!\n");
		//isolate and device reset
		writel(0xE8, DEV1_CFG);
		writel(0xE8, DEV2_CFG);
		writel(0xE8, DEV3_CFG);
		//set device0 to Host mode
		writel(0xa, DEV1_CFG + UTMIP_CFG);
		//release PHY reset and switch to Host mode
		writel(0xd9d9, VHUB_CFG);
		//release device0 reset，and DP、DM changed
		writel(0xe9, DEV1_CFG);
		//reset HUB and PHY to a confirmed state again
		writel(0x41d9, VHUB_CFG);
		//set device to device mode
		writel(0x26, DEV1_CFG + UTMIP_CFG);
		writel(0x26, DEV2_CFG + UTMIP_CFG);
		writel(0x26, DEV3_CFG + UTMIP_CFG);
		//reset device0 to confirmed state
		writel(0xe8, DEV1_CFG);
		mdelay(1);
		/* 
		 * switch to HUB mode,release HUB and PHY reset
		 * DP and DM changed,allowing HUB to be enumerated
		 */
		//bit[4]:0->usb phy clock is 25M,bit[0]:0->hub
		writel(0x59c8, VHUB_CFG);
		//release device reset
		writel(0xe9, DEV1_CFG);
		writel(0xe9, DEV2_CFG);
		writel(0xe9, DEV3_CFG);
	} else {
		p_printf("vhub hose set!\n");
		//bit[4]:0->usb phy clock is 25M,bit[0]:1->host
		writel(0xd9c9, VHUB_CFG);
		writel(0x69, DEV1_CFG);
		writel(0x69, DEV2_CFG);
		writel(0x69, DEV3_CFG);

		writel(0xa, DEV1_CFG + UTMIP_CFG);
		writel(0xa, DEV2_CFG + UTMIP_CFG);
		writel(0xa, DEV3_CFG + UTMIP_CFG);
	}
	//KEEP ALIVE compatible mouse and keyboard
	writel(readl(VHUB_USB2_0_PHY + CONFIG_VHUB_USB2_0_PHY) | 0x80000, VHUB_USB2_0_PHY + CONFIG_VHUB_USB2_0_PHY);
}

/* configure core security level */
void core_security_level_cfg(void)
{
	struct arm_smccc_res res;
	//level: 0, 1, 2
	uint8_t level_cfg = 0x0;

	printf("security level cfg...\n");
	arm_smccc_smc(CPU_SECURITY_CFG, 0, level_cfg, 0, 0, 0, 0, 0, &res);
	if (res.a0 != 0) {
		printf("error ret  %lx\n", res.a0);
		while (1);
	}
}
#endif

#ifdef PHYTIUM_ETH_TRAIN
static void ft_just_mdio_write(unsigned long base_reg, uint8_t phy_addr, uint8_t reg, uint16_t value)
{
	unsigned long netctl;
	unsigned long netstat;
	unsigned long frame;

	netctl = ft_just_readl(base_reg, NCR); //read network control register
	netctl |= FT_BIT(MPE); //enable management port
	ft_just_writel(base_reg, NCR, netctl);

	//construct the frame for reading from the PHY
	frame = (FT_BF(SOF, 1)
		| FT_BF(RW, 1)
		| FT_BF(PHYA, phy_addr)
		| FT_BF(REGA, reg)
		| FT_BF(CODE, 2)
		| FT_BF(DATA, value));
	ft_just_writel(base_reg, MAN, frame); //write the frame to management register

	do {
		netstat = ft_just_readl(base_reg, NSR);
	} while (!(netstat & FT_BIT(IDLE))); //wait until the idle bit is set

	netctl = ft_just_readl(base_reg, NCR); //read network control reg
	netctl &= ~FT_BIT(MPE); //disable management port
	ft_just_writel(base_reg, NCR, netctl);
}

/* read the value of a register from the eth PHY via MDIO*/
static uint16_t ft_just_mdio_read(unsigned long base_reg, uint8_t phy_addr, uint8_t reg)
{
	unsigned long netctl;
	unsigned long netstat;
	unsigned long frame;

	netctl = ft_just_readl(base_reg, NCR); //read network control register
	netctl |= FT_BIT(MPE); //enable management port
	ft_just_writel(base_reg, NCR, netctl);

	//construct the frame for reading from the PHY
	frame = (FT_BF(SOF, 1)
		| FT_BF(RW, 2)
		| FT_BF(PHYA, phy_addr)
		| FT_BF(REGA, reg)
		| FT_BF(CODE, 2));
	ft_just_writel(base_reg, MAN, frame); //write the frame to management register

	do {
		netstat = ft_just_readl(base_reg, NSR);
	} while (!(netstat & FT_BIT(IDLE))); //wait until the idle bit is set

	frame = ft_just_readl(base_reg, MAN); //read the frame back
	netctl = ft_just_readl(base_reg, NCR); //read network control reg
	netctl &= ~FT_BIT(MPE); //disable management port
	ft_just_writel(base_reg, NCR, netctl);

	return FT_BFEXT(DATA, frame);
}

/* Phytium eth sgmii mode training */
static int phytium_eth_sgmii_training(uint32_t mac_base, uint32_t phy_base)
{
	uint32_t tx_pkts, rx_pkts, symbol, fcs;
	uint32_t phy_rst_offset = 0;
	uint16_t rec_data;
	int i, j, ret, ret1, count;

	p_printf("SGMII_TEST_START\n");
	p_printf("eth ctrl = 0x%x, phy = 0x%x\n", mac_base, phy_base);
	count = 0, ret1 = 0, j = 0;
	rec_data = ft_just_mdio_read(mac_base, 0, 0x0);
	//enter sgmii_digital_loopack, 4140 1000M, 6100 100M, 4100 10M
	ft_just_mdio_write(mac_base, 0, 0x0, 0x4140);
	for (i = 0; i < 10; i++) {
		debug("FT_FRAMES_TXED_OK 0x%x\n", readl((size_t)(mac_base + FT_FRAMES_TXED_OK)));
		debug("FT_FRAMES_RXED_OK 0x%x\n", readl((size_t)(mac_base + FT_FRAMES_RXED_OK)));
		debug("FT_FCS_ERRORS 0x%x\n", readl((size_t)(mac_base + FT_FCS_ERRORS)));

		//config MAC
		writel(0x0, (size_t)(mac_base + UPPER_RX_Q_BASE_ADDR)); //init rx buffer upper 32-bit addr
		writel(0x00000030, (size_t)(mac_base + FT_NCR)); //init network control reg
		writel(TRAINING_RX_DESC + 0x00, (size_t)(mac_base + RECEIVE_Q_PTR));//config DMA rx descriptor addr
		writel(0x0, (size_t)(mac_base + UPPER_TX_Q_BASE_ADDR)); //init tx buffer upper 32-bit addr
		writel(TRAINING_TX_DESC + 0x00, (size_t)(mac_base + TRANSMIT_Q_PTR)); //config DMA tx descriptor addr
		writel(0x0c542c1a, (size_t)(mac_base + FT_CFG)); //set network config reg
		writel(0x40180f10, (size_t)(mac_base + FT_DMA_CFG)); //set DMA configuraton reg
		writel(0x1140, (size_t)(mac_base + PCS_CONTROL)); //enable auto-negotiation
		writel(0xffffffff, (size_t)(mac_base + INT_ENABLE)); //enable interrupt
		writel(0x0000001c, (size_t)(mac_base + FT_NCR));
		isb();

		debug("FT_NSR 0x%x\n", readl((size_t)(mac_base + FT_NSR)));
		//rgmii dosen't need training
		while (!(0x1 & readl((size_t)(mac_base + FT_NSR)))) {
			mdelay(5);
			j++;
			if (j > 10) {
				ret = 0xff;
				goto training_exit;
			}
		}
		debug("FT_NSR 0x%x, j:%d\n", readl((size_t)(mac_base + FT_NSR)), j);

		/* rx buffer */
		memset((void *)(TRAINING_RX_BUFFER + BUFFER0_OFFSET), 0, 0x500);
		/* tx buffer */
		memset((void *)(TRAINING_TX_BUFFER + BUFFER0_OFFSET), 0, 0x60);

		/* fill tx buf0 AA55 */
		writel(0xffffffff, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x00);
		writel(0x0000ffff, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x04);
		writel(0x10301200, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x08);
		writel(0x00450008, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x0c);
		writel(0x5e003600, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x10);
		writel(0xfd800000, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x14);
		writel(0xa8c010b8, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x18);
		writel(0xa8c00200, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x1c);
		writel(0xaa000a00, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x20);
		writel(0xaa55aa55, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x24);
		writel(0xaa55aa55, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x28);
		writel(0xaa55aa55, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x2c);
		writel(0xaa55aa55, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x30);
		writel(0xaa55aa55, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x34);
		writel(0xaa55aa55, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x38);
		writel(0xaa55aa55, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x3c);
		writel(0xaa55aa55, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x40);
		writel(0x019b51d6, TRAINING_TX_BUFFER + BUFFER0_OFFSET + 0x44);

		/* fill tx buf1 ff-00 */
		writel(0xffffffff, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x00);
		writel(0x0000ffff, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x04);
		writel(0x10301200, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x08);
		writel(0x00450008, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x0c);
		writel(0x74003600, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x10);
		writel(0xfd800000, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x14);
		writel(0xa8c0fab7, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x18);
		writel(0xa8c00200, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x1c);
		writel(0xff000a00, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x20);
		writel(0xff00ff00, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x24);
		writel(0xff00ff00, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x28);
		writel(0xff00ff00, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x2c);
		writel(0xff00ff00, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x30);
		writel(0xff00ff00, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x34);
		writel(0xff00ff00, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x38);
		writel(0xff00ff00, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x3c);
		writel(0xff00ff00, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x40);
		writel(0xaab94c62, TRAINING_TX_BUFFER + BUFFER1_OFFSET + 0x44);

		/* fill tx buf2 ffff */
		writel(0xffffffff, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x00);
		writel(0x0000ffff, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x04);
		writel(0x10301200, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x08);
		writel(0x00450008, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x0c);
		writel(0xbe013600, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x10);
		writel(0xfd800000, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x14);
		writel(0xa8c0b0b6, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x18);
		writel(0xa8c00200, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x1c);
		writel(0xff0f0a00, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x20);
		writel(0xffffffff, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x24);
		writel(0xffffffff, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x28);
		writel(0xffffffff, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x2c);
		writel(0xffffffff, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x30);
		writel(0xffffffff, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x34);
		writel(0xffffffff, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x38);
		writel(0xffffffff, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x3c);
		writel(0xffffffff, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x40);
		writel(0x4615e68b, TRAINING_TX_BUFFER + BUFFER2_OFFSET + 0x44);

		/* fill tx buf3 prbs */
		writel(0xffffffff, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x00);
		writel(0x0000ffff, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x04);
		writel(0x10301200, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x08);
		writel(0x00450008, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x0c);
		writel(0x2c003600, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x10);
		writel(0xfd800000, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x14);
		writel(0xa8c042b8, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x18);
		writel(0xa8c00200, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x1c);
		writel(0x5d9e0a00, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x20);
		writel(0xc12460d9, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x24);
		writel(0x47529605, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x28);
		writel(0xbe0ef580, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x2c);
		writel(0xd7849ef6, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x30);
		writel(0xd4c4d744, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x34);
		writel(0x55c7e8c4, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x38);
		writel(0xb148f5fb, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x3c);
		writel(0x5d937a1a, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x40);
		writel(0xc9868dc4, TRAINING_TX_BUFFER + BUFFER3_OFFSET + 0x44);

		/* rx desc fill*/
		for (j = 0; j < 4000; j++) {
			writel(TRAINING_RX_BUFFER + (0x80 * j), (size_t)(TRAINING_RX_DESC + (0x10 * j)));
			writel(0x0, (size_t)(TRAINING_RX_DESC + 0x04 + (0x10 * j)));
			writel(0x0, (size_t)(TRAINING_RX_DESC + 0x08 + (0x10 * j)));
		}
		isb();

		/* tx desc fill*/
		for (j = 0; j < 4000; j++) {
			writel(0x0, (size_t)(TRAINING_TX_DESC + 0x08 + (0x10 * j)));
			writel((TRAINING_TX_BUFFER + 0x80 * (j / 1000)), (size_t)(TRAINING_TX_DESC + 0x00 + (0x10 * j)));
			writel(0x18048, (size_t)(TRAINING_TX_DESC + 0x04 + (0x10 * j)));
		}
		writel(0x80000000, (size_t)(TRAINING_TX_DESC + 0x04 + j * 0x10));
		isb();

		/* send start */
		writel(0x21c, (size_t)(mac_base + FT_NCR));
		j = 0;
		isb();

		while (0x1 != (readl((size_t)(TRAINING_RX_DESC + 0x10 * 3999)) & 0xf)) {
			udelay(50);
			j++;
			if (j > 200000) {
				ret1 |= 1;
				break;
			}
		}

		while (0x80000000 != (readl((size_t)(TRAINING_TX_DESC + 0x10 * 3999 + 0x4)) & 0xf0000000)) {
			udelay(50);
			j++;
			if (j > 300000) {
				ret1 |= 2;
				break;
			}
		}

		tx_pkts = readl((size_t)(mac_base + FT_FRAMES_TXED_OK)); //get correct number of packets to be sent
		rx_pkts = readl((size_t)(mac_base + FT_FRAMES_RXED_OK)); //get correct number of packets to recv
		symbol = readl((size_t)(mac_base + FT_RX_SYMBOL_ERRORS)); //get recv packets counts when rx_er is valid
		fcs = readl((size_t)(mac_base + FT_FCS_ERRORS)); //get packet ckeck sequence errors
		isb();
		if (tx_pkts == rx_pkts) {
			if (symbol | fcs) {
				p_printf("error pkts fcs:0x%x symbol:0x%x\n", fcs, symbol);
				ret1 |= 4;
			}
		} else {
			ret1 |= 8;
		}
		p_printf("tx_pkts 0x%x, rx_pkts 0x%x\n", tx_pkts, rx_pkts);

		ret = ret1;
		/* close mdio, send and receive interface */
		writel(0x00000000, (size_t)(mac_base + FT_NCR));
		isb();

training_exit:

		if (ret) {
			p_printf("RST: SEL_TEST_FAIL %d\n", ret);
			count++;
			//If count is greater than 5 times, it may be risky
			if (count == 6) {
				p_printf("RST: SEL_TEST_ERROR\n");
				//Restore the initial value
				ft_just_mdio_write(mac_base, 0, 0x0, rec_data);
				return 1;
			}

			if (mac_base == GMAC1_BASE_ADDR)
				phy_rst_offset = 0x4008c;
			else
				phy_rst_offset = 0x40254;

			writel(0x00000000, (size_t)(phy_base + phy_rst_offset));
			mdelay(20);
			writel(0x00000001, (size_t)(phy_base + phy_rst_offset));
			mdelay(10);
		} else {
			//rx_buf
			memset((void *)(TRAINING_RX_BUFFER + BUFFER0_OFFSET), 0, 0x80 * 1000);
			//tx_buf
			memset((void *)(TRAINING_TX_BUFFER + BUFFER0_OFFSET), 0, 0x80 * 4);
			p_printf("SEL_SGMII_SUCCESS\n");
			//Restore the initial value
			ft_just_mdio_write(mac_base, 0, 0x0, rec_data);
			break;
		}
	}

	p_printf("RST: SEL_TEST_PASS\n");

	return 0;
}

void phytium_eth_training(void)
{
	uint32_t mode;
	uint32_t phy_sel;
	uint32_t mac_base;
	uint32_t phy_base;
	int i;
	int temp;

	/*
	 * Determine whether the flag of ShareMemory is 0x6, if so,
	 * it is an eco chip, and eth training is not required
	 */
	temp = readl(SHARE_MEM_ETH_TRAINING);
	if (((temp >> 0x1) & 0x1e) == 0x6)
		return;

	mode = pm_get_mac_mode_all() << 8;
	phy_sel = pm_get_phy_sel_all() & 0xffffffff;

	//the eth max count is 4
	for (i = 0; i < 4; i++) {
		if (!(phy_sel >> (4 + (i * 2)) & 0x3)) {
			if ((mode >> (8 + (i * 4)) & 0x3) == 0x3) {
				mac_base = GMAC0_BASE_ADDR + (i * 0x2000);
				phy_base = GSD_PHY0_BASE + (i * 0x100000);
				phytium_eth_sgmii_training(mac_base, phy_base);
			}
		}
	}
}

#endif
